import time, os, sys

from media.sensor import *
from media.display import *
from media.media import *
from machine import TOUCH
from machine import Pin

#导入串口模块
from machine import UART
from machine import FPIOA
import time

from collections import deque


posV = 1
lenV = posV*2 + 1
queX = [0]*lenV
queY = [0]*lenV
print(dir(queX))

def median_filter(que,v):
    for i in range(posV+1):
        if que[i] <= v:
            que.insert(i, v)
            que.pop()
            return que[posV]

        pos2 = lenV-i-1
        if que[pos2] >= v:
            que.insert(lenV-1, v)
            que.pop(0)
            return que[posV]



##############################################################
adjustState = 0
thdH = 67
def saveCfg():
    global thdH
    f = open('/sdcard/1.txt', 'w') #以写的方式打开一个文件，没有该文件就自动新建
    f.write(str(thdH)) #写入数据
    f.close()

def loadCfg():
    global thdH
    try:
        f = open('/sdcard/1.txt', 'r') #以读方式打开一个文件
        text = f.read()
        thdH = int(text)
        f.close()
    except BaseException as e:
        thdH = 67
        saveCfg()

loadCfg()

tp = TOUCH(0)
def TouchProcess():
    global thdH
    p = tp.read()
    if p!=():
        cp = p[0]
        thdH = int( min(255,max(0,cp.y-20)*255/400) )

KEY=Pin(21,Pin.IN,Pin.PULL_UP) #构建KEY对象
keyState = 0
def KeyProcess():
    global adjustState,keyState
    if keyState == 0:
        if KEY.value()==0:   #按键被按下
            time.sleep_ms(20) #消除抖动
            if KEY.value()==0: #确认按键被按下
                keyState = 1
    else:
        if KEY.value()==1:
            if adjustState == 0: adjustState = 1
            else:
                saveCfg()
                adjustState = 0
            keyState = 0

######################################################################
DETECT_WIDTH = 640
DETECT_HEIGHT = 480
fps = time.clock()

sensor = Sensor( width = DETECT_WIDTH, height = DETECT_HEIGHT, fps=30 )
sensor.reset()
sensor.set_hmirror(False)
sensor.set_vflip(False)
sensor.set_framesize(width = DETECT_WIDTH, height = DETECT_HEIGHT)
sensor.set_pixformat(Sensor.RGB888) #RGB565 GRAYSCALE
fpioa = FPIOA()

# UART1代码
fpioa.set_function(11,FPIOA.UART2_TXD)
fpioa.set_function(12,FPIOA.UART2_RXD)

uart=UART(UART.UART2,9600) #设置串口号2和波特率




Display.init(Display.ST7701, width=640, height=480, to_ide=False)
#Display.init(Display.VIRT, width=DETECT_WIDTH, height=DETECT_HEIGHT, to_ide=True)
MediaManager.init()
sensor.run()

cx = 0
cy = 0
img2 = sensor.snapshot()
nump2 = img2.to_numpy_ref()

img  = img2.to_grayscale()
nump = img.to_numpy_ref()
nump[:,:] = nump2[:,:,2]
while True:
    fps.tick()

    KeyProcess()
    if adjustState: TouchProcess()

    img2 = sensor.snapshot()
    nump[:,:] = nump2[:,:,2]
    #Display.show_image(img)
    #continue

    img.binary([(0, thdH)]) #需要调参
    img.mean(2, threshold=True, offset=10, invert=True)
    rs = img.find_rects(threshold = 200000)
    #print(len(rs))
    detected = False
    for r in rs:
        x = r.x()
        y = r.y()
        w = r.w();
        h = r.h();
        c = r.corners()
        c0 = c[0]
        c1 = c[1]
        c2 = c[2]
        c3 = c[3]

        if x>0 and y>0 and w>40 and h>30 and w< 640 and h<480:
            if w * h > 10000 and w > h and w < h*1.55:
                dy = abs( c0[1] - c1[1] )
                dx = abs( c1[0] - c2[0] )
                #print(dx,dy,w,h)
                if dy < h*0.2 and dx < w*0.2:
                    #print([r,c])
                    cx = r.x() + (w>>1)  #median_filter(queX,r.x() + (w>>1) )
                    cy = r.y() + (h>>1)  #median_filter(queY,r.y() + (h>>1) )
                    detected = True
                    break

    if detected:
        uart.write(f"[{cx},{cy}]")
        img.draw_circle(cx, cy, 16, color = (0, 255, 0))
    else: uart.write('[999,999]')

    if adjustState == 1 : img.draw_string_advanced(0, 0, 32, str(thdH) )
    Display.show_image(img)

